// #include <RotaryEncoder.h>//for left & right encoder
#include <Arduino.h>
#include <Timer.h>

// #include <PinChangeInt.h>
#include "parameters.hpp"
#include "mobile.hpp"

#define ENABLE_MOTOR_CTRL
// #define ENABLE_CLIFF
#define FREQ_FEEDBACK      (20) //, 20ms
#define FREQ_BATTARY       (1000 * 60 * 10) //, 10 minute
#define BATTARY_ANALOG     (A0)

Parameters  parameters;
Mobile      mobile;

Timer       timer_feedback;//feedba k 50Hz
// Timer       timer_getBattery;

void funcFeedback() {
  //用红外作为bumper 0代表障碍 
  //mobile.setBumper(digitalRead(BUMPER_LEFT), digitalRead(BUMPER_CENTER) | digitalRead(BUMPER_CENTER1), digitalRead(BUMPER_RIGHT));

#ifdef ENABLE_MOTOR_CTRL
  mobile.move();
  mobile.getEncoderFromMotor(LEFT_MOTOR);
  mobile.getEncoderFromMotor(RIGHT_MOTOR);
#endif

#ifdef ENABLE_CLIFF
  mobile.setCliff(A1, A2, A3);
#endif

}

void funcGetBattery()
{
  /*read battery volt*/  
  mobile.setBattery(analogRead(BATTARY_ANALOG));
}

void setup() {
  Serial.begin(115200);//commu to computer
  Serial.setTimeout(1);
  delay(100);
  Serial1.begin(115200);//commu to roadVehical to get yaw
  Serial1.setTimeout(1);
  delay(100);

  mobile.init(parameters);
  mobile.enable();
  timer_feedback.every(FREQ_FEEDBACK, funcFeedback);
  timer_feedback.every(FREQ_BATTARY, funcGetBattery);
  // timer_getBattery.every(FREQ_BATTARY, funcGetBattery);

  mobile.setBattery(analogRead(BATTARY_ANALOG));
}


void loop() {
  mobile.funcFeedback();
  timer_feedback.update();
  // timer_getBattery.update();
}

void serialEvent() {
  mobile.run();
}

void serialEvent1() {
  mobile.getInertia();
}
